#include "pairwise_distance_neighborhood.h"

#include <malloc.h>
#include <assert.h>

#include <math.h>

//#include "../pdb_index/xyz.h"

#define xorSwap(x,y) {x=x^y; y=x^y; x=x^y;}

void pairwise_distance_neighborhood_calculate(short paird[], short sorted[], void  * center, char * positions, short count, int objectSize, double (*distance)(const void*,const void*)) {
  short i;
  short j;
  size_t paird_position;
  short * first_row_distances = malloc(sizeof(short) * (count));
  //  short * sorted = malloc(sizeof(short) * (count));
  
  //  assert(count > 0);

  for( i = 0; i < count; i++) {
    sorted[i] = i;
    first_row_distances[i] = (short) (distance(center, &(positions[i * objectSize])) / 10.0);
    assert(first_row_distances[i] >= 0);
  }

  for(i = 0; i < count; i++) {
    for(j = 0; j < count-1; j++) {
      if(first_row_distances[sorted[j]] >  
	 first_row_distances[sorted[j+1]]) {
	xorSwap(sorted[j], sorted[j+1]);
      }
    }
  }
  
  for(i = 1; i < count; i++) {
    //    printf("pos = %d, dist = %d\n", (int) sorted[i-1], (int)first_row_distances[sorted[i-1]]);
    assert (first_row_distances[sorted[i-1]] <= first_row_distances[sorted[i]]);
  }
  //  printf("pos = %d, dist = %d\n", (int) sorted[i-1], (int)first_row_distances[sorted[i-1]]);
  paird_position = 0;
  
  for( i = 0; i < count; i++) {
    paird[paird_position] = first_row_distances[sorted[i]];
    paird_position++;
  }
  
  for( i = 0; i < count; i++) {
    for( j = i+1; j < count; j++) {
      paird[paird_position] = (short)(distance(&(positions[ objectSize * sorted[i]]),&(positions[ objectSize * sorted[j]]))/10.0);
      //      printf("%d\n", paird_position);
      assert(paird[paird_position] >= 0);
      paird_position++;
    }
  }
  
  assert(paird_position == pairwise_distance_neighborhood_get_array_size(count));

  free(first_row_distances);
  //  free(sorted);

}


double pairwise_distance_neighborhood_distances(short a[], short b[], short array_size) {
  double ret = 0.0;

  short i;
  for(i = 0; i < array_size; i++) {
    double t = (double)(a[i]-b[i]);
    ret += t * t;
  }
  
  return sqrt(ret);

}

int pairwise_distance_neighborhood_get_array_size(short count) {
  return (count * (1+count))/2;
}
